#include "Angle.h"

static void
normalize (vector<vector<double> > & ala,
	   const vector<double > & box)
{
  // if (ala.size() <= 1){
  //   return ;
  // }
  for (unsigned ii = 1; ii < ala.size() ; ++ii){
    if      (ala[ii][0] - ala[0][0] > 0.5 * box[0]) ala[ii][0] -= box[0];
    else if (ala[ii][0] - ala[0][0] <-0.5 * box[0]) ala[ii][0] += box[0];
    if      (ala[ii][1] - ala[0][1] > 0.5 * box[1]) ala[ii][1] -= box[1];
    else if (ala[ii][1] - ala[0][1] <-0.5 * box[1]) ala[ii][1] += box[1];
    if      (ala[ii][2] - ala[0][2] > 0.5 * box[2]) ala[ii][2] -= box[2];
    else if (ala[ii][2] - ala[0][2] <-0.5 * box[2]) ala[ii][2] += box[2];
  }
}

static vector<double>
crossProd (const vector<double> & diff0,
	   const vector<double> & diff1)
{
  vector<double> re (3);
  re[0] = (diff0[1] * diff1[2] - diff0[2] * diff1[1]);
  re[1] =-(diff0[0] * diff1[2] - diff0[2] * diff1[0]);
  re[2] = (diff0[0] * diff1[1] - diff0[1] * diff1[0]);  
  return re;
}

static double
dotProd (const vector<double> & diff0,
	 const vector<double> & diff1)
{
  return (diff0[0] * diff1[0] +
	  diff0[1] * diff1[1] +
	  diff0[2] * diff1[2] );
}

static vector<double>
calNormVec (vector<vector<double> > & group)
{
  vector<double> diff0 (3), diff1(3);

  for (unsigned dd = 0; dd < 3; ++dd){
    diff0[dd] = group[0][dd] - group[1][dd];
    diff1[dd] = group[2][dd] - group[1][dd];
  }

  return crossProd (diff0, diff1);
}

static double
calAngle_ (const vector<double> aa,
	   const vector<double> bb)
{
  double ep = 1e-8;
  double la = aa[0] * aa[0] + aa[1] * aa[1] + aa[2] * aa[2];
  double lb = bb[0] * bb[0] + bb[1] * bb[1] + bb[2] * bb[2];
  la = sqrt (la);
  lb = sqrt (lb);
  if (la < ep || lb < ep){
    return -1000;
  }
  double cosv = dotProd(aa, bb) / la / lb;
  if (cosv > 1) {
    cerr << "cos value " << cosv << " >  1" << endl << endl;;
    cosv = 1;
  }
  else if (cosv < -1){
    cerr << "cos value " << cosv << " < -1" << endl << endl;    
    cosv = -1;
  }
  return acos (cosv) / M_PI * 180.;
}

AngleCalculator::
AngleCalculator (const vector<double > & box_)
    : box (box_)
{
}


double AngleCalculator::
calAngle (const vector<vector<double> > & xx)
{
  vector<vector<double> > tmpxx(xx);
  normalize (tmpxx, box);

  if (xx.size() < 4){
    cerr << "size of vector is smaller than 4" << endl;
    exit (1);
  }
  vector<double> aa(3), bb(3), cc(3), bond(3);
  vector<vector<double> > group(3);
  
  group[0] = xx[0];
  group[1] = xx[1];
  group[2] = xx[2];
  aa = calNormVec (group);
  group[0] = xx[1];
  group[1] = xx[2];
  group[2] = xx[3];
  bb = calNormVec (group);

  if (dotProd(aa, aa) < 1e-16 || dotProd (bb,bb) < 1e-16){
    cerr << "small cross prod detacted! strange!" << endl;
  }
  double psi = calAngle_ (aa, bb);
  if (psi < -999){
    cerr << "wrong psi detected!" << endl;
  }
  cc = crossProd (aa, bb);
  for (unsigned dd = 0; dd < 3; ++dd){
    bond[dd] = xx[2][dd] -  xx[1][dd];
  }
  if (dotProd (cc, bond) < 0) psi = -psi;

  return psi;
}


void AngleCalculator::
calGrad (const vector<vector<double> > & xx,
	 const double & dx,
	 vector<vector<double> > & grad)
{
  for (unsigned ii = 0; ii < 4; ++ ii){
    vector<double > tmpgrad (3);
    for (unsigned dd = 0; dd < 3; ++dd){
      vector<vector<double > > xx0 (xx);
      vector<vector<double > > xx1 (xx);
      xx0[ii][dd] += dx;
      xx1[ii][dd] -= dx;
      double phi0 = calAngle (xx0);
      double phi1 = calAngle (xx1);
      if ((phi1 - phi0) > 180.){
	phi1 -= 360.;
      }
      else if (phi1 - phi0 < -180){
	phi1 += 360;
      }
      tmpgrad[dd] = (phi1 - phi0) / (2. * dx);
    }
    grad.push_back (tmpgrad);
  }  
}

double AngleCalculator::
calLaplacian (const vector<vector<double> > & xx,
	      const double & dx)
{
  double sum = 0.;
  double phi = calAngle (xx);
  for (unsigned ii = 0; ii < 4; ++ii){
    for (unsigned dd = 0; dd < 3; ++dd){
      vector<vector<double > > xx0 (xx);
      vector<vector<double > > xx1 (xx);
      xx0[ii][dd] += dx;
      xx1[ii][dd] -= dx;
      double phi0 = calAngle (xx0);
      double phi1 = calAngle (xx1);
      if ((phi1 - phi) > 180.){
	phi1 -= 360.;
      }
      else if (phi1 - phi < -180){
	phi1 += 360;
      }
      if ((phi0 - phi) > 180.){
	phi0 -= 360.;
      }
      else if (phi0 - phi < -180){
	phi0 += 360;
      }
      sum += ((phi1 - 2. * phi + phi0) / (dx * dx));
    }
  }
  return sum;
}

	 
